using System.Text;
using TPXSDK.NET.GIS;

namespace TPXSDK.NET.Mission;

/// <summary>
/// 任务指令工具类
/// </summary>
public static class FlyUtils
{
    /// <summary>
    /// 从字符串中解析出绝对位置数据, 解析失败返回AbsPos.Zero
    /// </summary>
    public static AbsPos Str2AbsPos(string strData)
    {
        try
        {
            strData = strData.Trim();
            var result = new AbsPos();
            if (strData.StartsWith('(') &&
                strData.EndsWith(')'))
            {
                strData = strData[1..^1].ToLower();// remove "()"
            }

            // 解析数据
            var strPos = strData.Split(',');

            strPos[0] = strPos[0].Trim();
            result.Lng = Convert.ToDouble(strPos[0][0..^1]);

            strPos[1] = strPos[1].Trim();
            result.Lat = Convert.ToDouble(strPos[1][0..^1]);

            if (strPos[0].EndsWith('w') || strPos[0].EndsWith('W')) result.Lng *= -1;
            if (strPos[1].EndsWith('s') || strPos[1].EndsWith('S')) result.Lat *= -1;

            return result;
        }
        catch (Exception)
        {
            return AbsPos.Zero;
        }
    }

    /// <summary>
    /// 将AbsPos对象转换成可以被任务编译器识别的字符格式(带后缀符)
    /// </summary>
    public static string AbsPos2Str(AbsPos pos, bool useAlt = false)
    {
        var sb = new StringBuilder();
        var lng = pos.Lng;
        var lat = pos.Lat;

        if (useAlt)
        {
            sb.Append((lng < 0) ? $"({-lng:F7}W," : $"({lng:F7}E,");
            sb.Append((lat < 0) ? $" {-lat:F7}S)," : $" {lat:F7}N),");
            sb.Append(pos.Alt.ToString("0.##"));
        }
        else
        {
            sb.Append((lng < 0) ? $"({-lng:F7}W," : $"({lng:F7}E,");
            sb.Append((lat < 0) ? $" {-lat:F7}S)" : $" {lat:F7}N)");
        }

        return sb.ToString();
    }

    /// <summary>
    /// 从字符串中解析出相对位置数据, 解析失败返回空数组
    /// </summary>
    public static double[] Str2ResPos(string strData)
    {
        try
        {
            var result = new double[2];
            if (strData.StartsWith('(') &&
                strData.EndsWith(')'))
            {
                strData = strData[1..^1];// 去除"()"
            }

            // 解析数据
            var strPos = strData.Split(',');
            result[0] = Convert.ToDouble(strPos[0].Trim());
            result[1] = Convert.ToDouble(strPos[1].Trim());
            return result;
        }
        catch (Exception)
        {
            return [];
        }
    }

    /// <summary>
    /// 从命令中解析位置数据的字符串表达方式
    /// </summary>
    internal static string ParseWpFromCmdItem(AnyValue p3, AnyValue p4)
    {
        var sb = new StringBuilder();
        var lng = p3.S32Val / 10e6;
        var lat = p4.S32Val / 10e6;
        sb.Append((lng < 0) ? $"({-lng:F7}W," : $"({lng:F7}E,");
        sb.Append((lat < 0) ? $" {-lat:F7}S)" : $" {lat:F7}N)");
        return sb.ToString();
    }

    internal static string ParseWpFromCmdItem(FlyCmdItem cmd)
    {
        var sb = new StringBuilder();
        var pt = (ParaType)(cmd.P1 & 0x0F);
        if (ParaType.REL_POS == pt)
        {
            double x = cmd.P3.FloatVal;
            double y = cmd.P4.FloatVal;
            sb.Append($"({x.ToString("0.###")},{y.ToString("0.###")})");
        }
        else if (ParaType.ABS_POS == pt)
        {
            var lng = cmd.P3.S32Val / 10e6;
            var lat = cmd.P4.S32Val / 10e6;
            sb.Append((lng < 0) ? $"({-lng:F7}W," : $"({lng:F7}E,");
            sb.Append((lat < 0) ? $" {-lat:F7}S)" : $" {lat:F7}N)");
        }
        else
        {
            return @"(?,?)";
        }
        return sb.ToString();
    }

    /// <summary>
    /// 将词元转换成对应的命令参数1中的数值
    /// </summary>
    internal static bool WpToken2CmdItemP1(Token token, FlyCmdItem flyCmdItem)
    {
        try
        {
            switch (token.Type)
            {
                case TokenType.REL_POS:
                    var posData = Str2ResPos(token.Name);
                    if (posData == null) return false;
                    flyCmdItem.P1 = (byte)ParaType.REL_POS;
                    flyCmdItem.P3.FloatVal = (float)posData[0];
                    flyCmdItem.P4.FloatVal = (float)posData[1];
                    break;
                case TokenType.ABS_POS:
                    var pos = Str2AbsPos(token.Name);
                    if (pos == AbsPos.Zero) return false;
                    flyCmdItem.P1 = (byte)ParaType.ABS_POS;
                    flyCmdItem.P3.S32Val = (int)(pos.Lng * 1e7);
                    flyCmdItem.P4.S32Val = (int)(pos.Lat * 1e7);
                    break;
                default:
                    return false;
            }
        }
        catch (Exception)
        {
            return false;
        }
        return true;
    }

    /// <summary>
    /// 无参数指令统一处理函数, 处理类似 xxx 形式的指令
    /// </summary>
    public static bool Process4NoPara(FlyCompiler flyCmp, int keyValue)
    {
        var tokens = flyCmp.GetLineTokens();
        if (tokens?.Count != 1) return false;
        var cmd = new FlyCmdItem
        {
            LineNum = tokens[0].Row,
            Id = (byte)keyValue,
            P1 = (byte)ParaType.EMPTY,
            P2 = (short)ParaType.EMPTY
        };
        flyCmp.CmdList.Add(cmd);

        return true;
    }

    /// <summary>
    /// 用于反编译无参数的Fly命令
    /// </summary>
    internal static string Reverse4NoPara(FlyCmdItem flyCmdItem, string keyName)
    {
        if (flyCmdItem.P1 == (byte)ParaType.EMPTY &&
            flyCmdItem.P2 == (byte)ParaType.EMPTY)
        {
            return keyName;
        }
        return $"{keyName} ???";
    }

    /// <summary>
    /// 判断是否为导航指令（参数可能带坐标信息）
    /// </summary>
    public static bool IsNavCmd(FlyCmdItem item)
    {
        return item.Id == (byte)FlyCmdId.CMD_FLYTO ||
               item.Id == (byte)FlyCmdId.CMD_LAND ||
               item.Id == (byte)FlyCmdId.CMD_CIRCLELEFT ||
               item.Id == (byte)FlyCmdId.CMD_CIRCLERIGHT;
    }

    /// <summary>
    /// 判断该命令是否为结束语句
    /// </summary>
    internal static bool IsEndCmd(int cmdId)
    {
        return cmdId == (int)FlyCmdId.CMD_REPEAT
               || cmdId == (int)FlyCmdId.CMD_LAND
               || cmdId == (int)FlyCmdId.CMD_RTL;
    }

    /// <summary>
    /// 获得指令包含的绝对坐标信息(包括海拔高度)
    /// </summary>
    public static AbsPos GetAbsPos(FlyCmdItem item, AbsPos refPos)
    {
        if (false == IsNavCmd(item)) return AbsPos.Zero;
        var pt = (ParaType)(item.P1 & 0x0F);// 增加对高位屏蔽支持

        if (ParaType.REL_POS == pt)
        {
            var res = refPos.LocOffset(item.P3.FloatVal, item.P4.FloatVal);
            res.Alt = refPos.Alt + item.P2;
            return res;
        }

        if (ParaType.ABS_POS == pt)
        {
            return new AbsPos(item.P3.S32Val / 1e7, item.P4.S32Val / 1e7, item.P2);
        }

        return AbsPos.Zero;
    }

    /// <summary>
    /// 更安全版本的数值类型转换，支持浮点数("12.345")转换为(12)整形
    /// </summary>
    public static short ToInt16(string str_val)
    {
        var d_val = Convert.ToDouble(str_val);
        return Convert.ToInt16(d_val);
    }
}
